#include "sgmdev_pd.h"
#include "sgmdev_charger.h"
#include "insdrv_gpio.h"
#include "insdrv_i2c.h"
#include "tcpci_event.h"
#include "tcpci.h"
#include "tcpci_core.h"
#include "tcpci_timer.h"
#include "tcpm.h"
#include "pd_core.h"

#undef LOG_TAG
#define LOG_TAG "sgm_pd"
#include "elog.h"

#if CHIP_TYPE == CHIP_SG41620
#define PD_DEV_ADDR    (0x52 <<1)
#define SGM_DEV_PID     SGM_41620_PID
#elif CHIP_TYPE == CHIP_SG795
#define PD_DEV_ADDR    (0x4E <<1)
#define SGM_DEV_PID     SGM_795_PID
#endif

#pragma pack(push, 1)
struct tcpc_transmit_packet {
    uint8_t cnt;
    uint16_t msg_header;
    uint8_t data[sizeof(uint32_t)*7];
};
#pragma pack(pop)

struct sgm41620_tcpc g_tcpc_dev;
struct sgm41620_tcpc *p_tcpc_dev = &g_tcpc_dev;
extern void delay_1ms(uint32_t n);
extern void delay_1us(uint32_t us);

int sgmdev_pd_read_reg(uint8_t reg, uint8_t len, uint8_t *dst)
{
	int ret = 0, count = 5;
	uint8_t buffer = 0;

	while (count) {
		if (len > 1) {
			ret = _i2c1_rcv(PD_DEV_ADDR, reg, dst, len);
			if (ret != 0)
				count--;
			else
				return ret;
		}
		else {
			ret = _i2c1_rcv(PD_DEV_ADDR, reg, &buffer, 1);
			if (ret != 0)
				count--;
			else {
				*(uint8_t *)dst = buffer;
				return ret;
			}
		}
		delay_1us(500);
	}

	return ret;
}

int sgmdev_pd_write_reg(uint8_t reg, uint8_t len, uint8_t *src)
{
	int ret = 0, count = 5;

	while (count) {
		if (len > 1) {
			ret = _i2c1_send(PD_DEV_ADDR, reg, src, len);
			if (ret != 0)
				count--;
			else
				return ret;
		}
		else {
			ret = _i2c1_send(PD_DEV_ADDR, reg, src, 1);
			if (ret != 0)
				count--;
			else
				return ret;
		}
		delay_1us(500);
	}

	return ret;
}

static int sgmdev_i2c_write8(uint8_t reg, uint8_t data)
{
    int ret;

    ret =  sgmdev_pd_write_reg(reg, 1, &data);
    if (ret) {
        log_e("pd i2c write8 Failed: reg=%02X, ret=%d\n", reg, ret);
        return ret;
    }

    return 0;
}

int sgmdev_i2c_write16(uint8_t reg, uint16_t data)
{
    int ret;
    ret = sgmdev_pd_write_reg(reg, 2, (uint8_t *)&data);
    if (ret) {
        log_e("pd i2c write16 Failed: reg=%02X, ret=%d\n", reg, ret);
        return ret;
    }

    return 0;
}

static int sgmdev_i2c_read8(uint8_t reg,uint8_t *data)
{
    int ret;

    ret = sgmdev_pd_read_reg(reg,1,data);
    if (ret) {
        log_e("pd i2c read8 Failed: reg=%02X, ret=%d\n", reg, ret);
        return ret;
    }

    return 0;
}

static int sgmdev_i2c_read16(uint8_t reg,uint16_t *data)
{
    int ret;

    ret = sgmdev_pd_read_reg(reg,2,(uint8_t *)data);
    if (ret) {
        log_e("pd i2c read16 Failed: reg=%02X, ret=%d\n", reg, ret);
        return ret;
    }

    return 0;
}

int sgmdev_command(uint8_t cmd)
{
    return sgmdev_i2c_write8(TCPC_V10_REG_COMMAND, cmd);
}

static int sgmdev_software_reset(void)
{
#if CHIP_TYPE == CHIP_SG41620
    return sgmdev_i2c_write8(SGM41620_REG_SWRESET, 1);
#elif CHIP_TYPE == CHIP_SG795
    return sgmdev_i2c_write8(SGM795_REG_SWRESET, 1);
#endif
}

int sgmdev_check_revision(void)
{
	uint16_t vid, pid, did;
	int ret;

	ret = sgmdev_i2c_read16(TCPC_V10_REG_VID, &vid);
	if (ret) {
		log_e( "read chip ID fail\n");
		return -1;
	} else {
		log_i("%s read device, VID=0x%04x\n", __func__, vid);
	}

	if (vid != SGM_DEV_VID) {
		log_i("%s failed, VID=0x%04x\n", __func__, vid);
		return -2;
	}

	ret = sgmdev_i2c_read16(TCPC_V10_REG_PID, &pid);
	if (ret) {
		log_e( "read product ID fail\n");
		return -3;
	} else {
		log_i("%s read device, PID=0x%04x\n", __func__, pid);
	}

	if (pid != SGM_DEV_PID) {
		log_i("%s failed, PID=0x%04x\n", __func__, pid);
		return -4;
	}

	ret = sgmdev_software_reset();
	if (ret)
		return -5;
	delay_1ms(1);

	ret = sgmdev_i2c_read16(TCPC_V10_REG_DID, &did);
	if (ret) {
		log_e( "read device ID fail\n");
		return -6;
	}

	return did;
}

extern InsDrv_Gpio_Level_e InsDrv_Gpio_BAT32G157_GetPinLevel(InsDrv_Gpio_Port_e port, InsDrv_Gpio_Pin_e pin);
void sgmdev_pd_work_handle(void)
{
	int regval = 0;
	InsDrv_Gpio_Level_e gpio_val;
	do{
		regval =  tcpci_alert();
		if(regval)
			break;
		gpio_val = InsDrv_Gpio_BAT32G157_GetPinLevel(INSDRV_GPIO_PORTC, INSDRV_GPIO_PIN_7);  //C7
	}while(gpio_val == INSDRV_GPIO_LEVEL_LOW);
}

#if CHIP_TYPE == CHIP_SG41620
static int sgm41620_init_alert_mask(void)
{
    uint16_t mask;

    mask = TCPC_V10_REG_ALERT_CC_STATUS | TCPC_V10_REG_ALERT_POWER_STATUS;

    /* Need to handle RX overflow */
    mask |= TCPC_V10_REG_ALERT_TX_SUCCESS | TCPC_V10_REG_ALERT_TX_DISCARDED
            | TCPC_V10_REG_ALERT_TX_FAILED
            | TCPC_V10_REG_ALERT_RX_HARD_RST
            | TCPC_V10_REG_ALERT_RX_STATUS
            | TCPC_V10_REG_RX_OVERFLOW;

    mask |= TCPC_REG_ALERT_FAULT;

    return sgmdev_i2c_write16(TCPC_V10_REG_ALERT_MASK, mask);
}

static int sgm41620_init_power_status_mask(void)
{
    const uint8_t mask = TCPC_V10_REG_POWER_STATUS_VBUS_PRES;

    return sgmdev_i2c_write8(TCPC_V10_REG_POWER_STATUS_MASK, mask);
}

static int sgm41620_init_fault_mask(void)
{
    const uint8_t mask = TCPC_V10_REG_FAULT_STATUS_VCONN_OV | \
                         TCPC_V10_REG_FAULT_STATUS_VCONN_OC;

    return sgmdev_i2c_write8(TCPC_V10_REG_FAULT_STATUS_MASK, mask);
}

static int sgm41620_init_rt_mask(void)
{
    uint8_t rt_mask = 0;


#if 1 
	rt_mask |= SGM41620_REG_ANAINT_VBUS_80;
#endif /* CONFIG_TCPC_VSAFE0V_DETECT_IC */

#if CONFIG_TYPEC_CAP_RA_DETACH
	if (p_tcpc_dev->tcpc_flags & TCPC_FLAGS_CHECK_RA_DETACHE)
		rt_mask |= SGM41620_REG_M_RA_DETACH;
#endif /* CONFIG_TYPEC_CAP_RA_DETACH */

//#if CONFIG_TYPEC_CAP_LPM_WAKEUP_WATCHDOG
#if 1
	//if (p_tcpc_dev->tcpc_flags & TCPC_FLAGS_LPM_WAKEUP_WATCHDOG)
		rt_mask |= SGM41620_REG_ANAINT_WAKEUP;
#endif	/* CONFIG_TYPEC_CAP_LPM_WAKEUP_WATCHDOG */

	return sgmdev_i2c_write8(SGM41620_REG_ANAMASK, rt_mask);
}

static int sgm41620_alert_status_clear(uint32_t mask)
{
    int ret;
    uint16_t mask_t1 = 0;
    uint8_t mask_t2 = 0;

    /* Write 1 clear */
    mask_t1 = (uint16_t) mask;
    //log_i("after mask_t1 = 0x%x, mask = %x\r\n",mask_t1,mask);
    if (mask_t1) {
        ret = sgmdev_i2c_write16(TCPC_V10_REG_ALERT, mask_t1);
        if (ret)
            return ret;
    }
    mask_t2 = mask >> 16;
    if (mask_t2) {
        ret = sgmdev_i2c_write8(SGM41620_REG_ANAINT, mask_t2);
        if (ret )
            return ret;
    }

    return 0;
}

static int sgm41620_tcpc_init(bool sw_reset)
{
    uint8_t temp = 0;

    /* UFP Both RD setting */
    /* DRP = 0, RpVal = 0 (Default), Rd, Rd */
    sgmdev_i2c_write8(TCPC_V10_REG_ROLE_CTRL,
        TCPC_V10_REG_ROLE_CTRL_RES_SET(0, 0, CC_RD, CC_RD));

    /* Disable Vconn over-voltage detect  */
    sgmdev_i2c_write8(TCPC_V10_REG_FAULT_CTRL,TCPC_V10_REG_FAULT_CTRL_DIS_VCONN_OV);

    /*
     * DRP Toggle Cycle : 51.2 + 6.4 * val (ms)
     * DRP Duty Ctrl : dcSRC: / 1024
     */
    sgmdev_i2c_write8(SGM41620_REG_DRP_TOGGLE_CYCLE, 4);
    sgmdev_i2c_write16(SGM41620_REG_DRP_DUTY_CTRL0, TCPC_NORMAL_RP_DUTY & 0xFF);
    sgmdev_i2c_write16(SGM41620_REG_DRP_DUTY_CTRL1, (TCPC_NORMAL_RP_DUTY >> 8) & 0x3);

	sgmdev_i2c_write8(TCPC_V10_REG_FAULT_STATUS,0xFF);    //Clear Fault status
	sgm41620_alert_status_clear(0xffffffff);
	sgm41620_init_power_status_mask();
	sgm41620_init_alert_mask();
	sgm41620_init_fault_mask();
	sgm41620_init_rt_mask();

	/* CK_300K from 320K, SHIPPING off, AUTOIDLE enable, TIMEOUT = 32ms */
	sgmdev_i2c_write8(SGM41620_REG_IDLE_CTRL,SGM41620_REG_IDLE_SET(0, 1, 1, 2));
	
	sgmdev_i2c_read8(SGM41620_REG_IDLE_CTRL,&temp);
	log_i("%s temp = %x\r\n",__func__, temp);
	return 0;
}

static int sgm41620_fault_status_vconn_0v()
{
	int ret;
	uint8_t buf;
	
	ret = sgmdev_i2c_read8(SGM41620_REG_ANACTRL1,&buf);
	if (ret)
		return ret;

	buf &= ~SGM41620_REG_ANACTRL1_VCONN_DISC;
	return sgmdev_i2c_write8(SGM41620_REG_ANACTRL1, buf);
}

static int sgm41620_fault_status_clear(uint8_t status)
{
	if (status & TCPC_V10_REG_FAULT_STATUS_VCONN_OV)
		sgm41620_fault_status_vconn_0v();

	return sgmdev_i2c_write8(TCPC_V10_REG_FAULT_STATUS, status);
}

static int sgm41620_get_alert_mask(uint32_t *mask)
{
	int ret;
	uint16_t buf;
#if 1//def CONFIG_TCPC_VSAFE0V_DETECT_IC
	uint8_t v2;
#endif

	ret = sgmdev_i2c_read16(TCPC_V10_REG_ALERT_MASK,&buf);
	if (ret)
		return ret;

	*mask = buf;

#if 1//def CONFIG_TCPC_VSAFE0V_DETECT_IC
	ret = sgmdev_i2c_read8(SGM41620_REG_ANAMASK,&v2);
	if (ret)
		return ret;

	*mask |= v2 << 16;
#endif

	return 0;
}

static int sgm41620_set_alert_mask(uint32_t mask)
{
	return sgmdev_i2c_write16(TCPC_V10_REG_ALERT_MASK, mask);
}

static int sgm41620_get_alert_status(uint32_t *alert)
{
    int ret;
    uint16_t buf;
#if 1//def CONFIG_TCPC_VSAFE0V_DETECT_IC
    uint8_t v2;
#endif

    ret = sgmdev_i2c_read16(TCPC_V10_REG_ALERT, &buf);
    if (ret) {
        log_e("%s get alert_status fail ret =  0x%x\n",__func__,ret);
        return ret;
    }
    *alert = buf;

#if 1 //def CONFIG_TCPC_VSAFE0V_DETECT_IC
    ret = sgmdev_i2c_read8(SGM41620_REG_ANAINT, &v2);
    if (ret) {
        log_e("%s get rt_int fail ret =  0x%x\n",__func__,ret);
        return ret;
    }
    *alert |= v2 << 16;
#endif

    return 0;
}

static int sgm41620_get_power_status(uint16_t *pwr_status)
{
    int ret;
    uint8_t data;

    ret = sgmdev_i2c_read8(TCPC_V10_REG_POWER_STATUS, &data);
    if (ret) {
        log_e("%s get power status fail ret =  0x%x\n",__func__,ret);
        return ret;
    }
    *pwr_status = 0;

#if 1
    if (data & TCPC_V10_REG_POWER_STATUS_VBUS_PRES)
        *pwr_status |= TCPC_REG_POWER_STATUS_VBUS_PRES;
#endif

#if 1
    ret = sgmdev_i2c_read8(SGM41620_REG_ANASTS,&data);
    if (ret) {
        log_e("%s get rt status fail ret =  0x%x\n",__func__,ret);
        return ret;
    }

    if (data & SGM41620_REG_ANASTS_VBUS_80)
        *pwr_status |= TCPC_REG_POWER_STATUS_EXT_VSAFE0V;
#endif

    return 0;
}

static int sgm41620_get_fault_status(uint8_t *status)
{
    int ret;
    uint8_t buff;

    ret = sgmdev_i2c_read8(TCPC_V10_REG_FAULT_STATUS,&buff);
    if (ret) {
        log_e("%s get fault status fail ret =  0x%x\n",__func__,ret);
        return ret;
    }
    *status = buff;

    return 0;
}

static int sgm41620_get_cc( int *cc1, int *cc2)
{
    int ret; 
    uint8_t status,role_ctrl, cc_role,tcpc_ctrl;
    bool act_as_drp, act_as_sink;

    ret = sgmdev_i2c_read8(TCPC_V10_REG_CC_STATUS,&status);
    if (ret) {
        log_e("%s get cc status fail ret =  0x%x\n",__func__,ret);
        return ret;
    }

    ret = sgmdev_i2c_read8(TCPC_V10_REG_ROLE_CTRL,&role_ctrl);
    if (ret) {
        log_e("%s get role fail ret =  0x%x\n",__func__,ret);
        return ret;
    }

    ret = sgmdev_i2c_read8(TCPC_V10_REG_TCPC_CTRL,&tcpc_ctrl);
    if (ret) {
        log_e("%s get tcpc fail ret =  0x%x\n",__func__,ret);
        return ret;
    }

    if (status & TCPC_V10_REG_CC_STATUS_DRP_TOGGLING) {
        *cc1 = TYPEC_CC_DRP_TOGGLING;
        *cc2 = TYPEC_CC_DRP_TOGGLING;
        return 0;
    }

    *cc1 = TCPC_V10_REG_CC_STATUS_CC1(status);
    *cc2 = TCPC_V10_REG_CC_STATUS_CC2(status);

    act_as_drp = TCPC_V10_REG_ROLE_CTRL_DRP & role_ctrl;

    if (act_as_drp) {
        act_as_sink = TCPC_V10_REG_CC_STATUS_DRP_RESULT(status);
    } 
    else {
        if (p_tcpc_dev->typec_polarity)
            cc_role = TCPC_V10_REG_CC_STATUS_CC2(role_ctrl);
        else
            cc_role = TCPC_V10_REG_CC_STATUS_CC1(role_ctrl);

        if (cc_role == TYPEC_CC_RP)
            act_as_sink = false;
        else
            act_as_sink = true;
    }

    /*
     * If status is not open, then OR in termination to convert to
     * enum tcpc_cc_voltage_status.
     */
    if (*cc1 != TYPEC_CC_VOLT_OPEN)
        *cc1 |= (act_as_sink << 2);

    if (*cc2 != TYPEC_CC_VOLT_OPEN)
        *cc2 |= (act_as_sink << 2);

    //polarity = TCPC_V10_REG_TCPC_CTRL_PLUG_ORIENT & tcpc_ctrl;
    
    //log_i("status = 0x%x,tcpc_ctrl = 0x%x,role_ctrl = 0x%x\n",status,tcpc_ctrl,role_ctrl);
	return 0;
}

static int sgm41620_enable_vsafe0v_detect(bool enable)
{
	uint8_t data;
	int ret = sgmdev_i2c_read8(SGM41620_REG_ANAMASK,&data);

	if (ret)
		return ret;

	if (enable)
		data |= SGM41620_REG_ANAINT_VBUS_80;
	else
		data &= ~SGM41620_REG_ANAINT_VBUS_80;
	sgmdev_i2c_write8(SGM41620_REG_ANAMASK, data);
	
	sgmdev_command(TCPM_CMD_ENABLE_VBUS_DETECT);
	return 0;
}

static int sgm41620_set_cc(int pull)
{
    int ret;
    uint8_t data;
    int rp_lvl = TYPEC_CC_PULL_GET_RP_LVL(pull), pull1, pull2;

    //log_i("set rp_lvl = %x,pull = %d\r\n",rp_lvl,pull);
    pull = TYPEC_CC_PULL_GET_RES(pull);
    if (pull == TYPEC_CC_DRP) {
        data = TCPC_V10_REG_ROLE_CTRL_RES_SET(1, rp_lvl, TYPEC_CC_RD, TYPEC_CC_RD);

        ret = sgmdev_i2c_write8(TCPC_V10_REG_ROLE_CTRL, data);

        if (ret == 0) {
            sgm41620_enable_vsafe0v_detect(false);
            ret = sgmdev_command(TCPM_CMD_LOOK_CONNECTION);
            // sgmdev_i2c_read8(0x1d,&data);
            // log_i("0x1d = %x",data);
        }
    }
    else {
        pull1 = pull2 = pull;

        if (pull == TYPEC_CC_RP && p_tcpc_dev->typec_is_attached_src) {
            if (p_tcpc_dev->typec_polarity)
                pull1 = TYPEC_CC_OPEN;
            else
                pull2 = TYPEC_CC_OPEN;
        }

        data = TCPC_V10_REG_ROLE_CTRL_RES_SET(0, rp_lvl, pull1, pull2);
        // log_i("set cc data = %x\r\n",data);
        ret = sgmdev_i2c_write8(TCPC_V10_REG_ROLE_CTRL, data);
    }

    return 0;
}

static int sgm41620_set_polarity(int polarity)
{
	int ret = 0;
	uint8_t data;	
	
	if (ret)
		return ret;

	ret = sgmdev_i2c_read8(TCPC_V10_REG_TCPC_CTRL,&data);
	if (ret)
		return ret;

	data &= ~TCPC_V10_REG_TCPC_CTRL_PLUG_ORIENT;
	data |= polarity ? TCPC_V10_REG_TCPC_CTRL_PLUG_ORIENT : 0;

	return sgmdev_i2c_write8(TCPC_V10_REG_TCPC_CTRL, data);
}

static int sgm41620_set_low_rp_duty(bool low_rp)
{
    if (low_rp) {
        return sgmdev_i2c_write16(SGM41620_REG_DRP_DUTY_CTRL0, TCPC_LOW_RP_DUTY);
    } else {
        sgmdev_i2c_write16(SGM41620_REG_DRP_DUTY_CTRL0, TCPC_NORMAL_RP_DUTY & 0xFF);
        return sgmdev_i2c_write16(SGM41620_REG_DRP_DUTY_CTRL1, (TCPC_NORMAL_RP_DUTY >> 8) & 0x3);
    }
}

static int sgm41620_set_vconn(int enable)
{
    int rv;
    int ret;
    uint8_t data;

    ret = sgmdev_i2c_read8(TCPC_V10_REG_POWER_CTRL, &data);
    if (ret) {
        log_e("%s get power fail ret =  0x%x\n",__func__,ret);
        return ret;
    }

    data &= ~TCPC_V10_REG_POWER_CTRL_VCONN;
    data |= enable ? TCPC_V10_REG_POWER_CTRL_VCONN : 0;

    rv = sgmdev_i2c_write8(TCPC_V10_REG_POWER_CTRL, data);
    if (ret) {
        log_e("%s set power fail ret =  0x%x\n",__func__,ret);
        return ret;
    }

#ifndef CONFIG_TCPC_IDLE_MODE
    rv = sgmdev_i2c_write8(SGM41620_REG_IDLE_CTRL,
        SGM41620_REG_IDLE_SET(0, 1, enable ? 0 : 1, 2));
#endif /* CONFIG_TCPC_IDLE_MODE */

    return rv;
}

static int sgm41620_set_low_power_mode(bool en, int pull)
{
    if (en) {
        // not support
    } else {
        sgm41620_enable_vsafe0v_detect(true);
    }

    return 0;
}

static int sgm41620_tcpc_deinit(void)
{
#if CONFIG_TCPC_SHUTDOWN_CC_DETACH
    sgm41620_set_cc(TYPEC_CC_DRP);
    sgm41620_set_cc(TYPEC_CC_OPEN);    
#else
    sgm41620_software_reset();
#endif	/* CONFIG_TCPC_SHUTDOWN_CC_DETACH */

    return 0;
}

static int sgm41620_set_msg_header(uint8_t power_role, uint8_t data_role)
{
	uint8_t msg_hdr = TCPC_V10_REG_MSG_HDR_INFO_SET(data_role, power_role);

	return sgmdev_i2c_write8(TCPC_V10_REG_MSG_HDR_INFO, msg_hdr);
}

static int sgm41620_set_rx_enable(uint8_t enable)
{
    int ret = 0;

    ret = sgmdev_i2c_write8(TCPC_V10_REG_RX_DETECT, enable);

    if ((ret == 0) && (!enable)) {
        ret = sgm41620_alert_status_clear(TCPC_REG_ALERT_RX_STATUS | \
                    TCPC_REG_ALERT_RX_HARD_RST | \
                    TCPC_REG_ALERT_RX_BUF_OVF);
    }

    return ret;
}

static int sgm41620_get_message( uint32_t *payload,
			uint16_t *msg_head, enum tcpm_transmit_type *frame_type)
{
	int rv;
	uint8_t type, cnt = 0;
	uint8_t buf[4];
    //uint8_t buf2[28];
	const uint16_t alert_rx =
		TCPC_V10_REG_ALERT_RX_STATUS|TCPC_V10_REG_RX_OVERFLOW;

	rv = sgmdev_pd_read_reg(TCPC_V10_REG_RX_BYTE_CNT, 4, buf);
	cnt = buf[0];
	type = buf[1];
	*msg_head = *(uint16_t *)&buf[2];

	/* TCPC 1.0 ==> no need to subtract the size of msg_head */
	if (!rv && cnt > 3) {
		cnt -= 3; /* MSG_HDR */
		rv = sgmdev_pd_read_reg(TCPC_V10_REG_RX_DATA, cnt,(uint8_t *) payload);
	}

    *frame_type = (enum tcpm_transmit_type) type;

    /* Read complete, clear RX status alert bit */
    sgm41620_alert_status_clear(alert_rx);

    return rv;
}

static int sgm41620_set_bist_carrier_mode(uint8_t pattern)
{
	/* Don't support this function */
	return 0;
}

#if CONFIG_USB_PD_RETRY_CRC_DISCARD
static int sgm41620_retransmit(void)
{
	return sgmdev_i2c_write8(TCPC_V10_REG_TRANSMIT,
			TCPC_V10_REG_TRANSMIT_SET(
			p_tcpc_dev->pd_retry_count, TCPC_TX_SOP));
}
#endif

static int sgm41620_set_force_discharge(bool en, int mv)
{
	if(en)
		sgm41620_charger_update_bits(0x08,1 << 5,1 << 5);

	return 0;
}

static int sgm41620_set_vbus(bool en)
{
    if (en)
        return sgm41620_enable_vbus();
    else
        return sgm41620_disable_vbus();
}

static int sgm41620_transmit(enum tcpm_transmit_type type, uint16_t header, const uint32_t *data)
{
	int rv;
	int data_cnt;
	struct tcpc_transmit_packet packet;

	if (type < TCPC_TX_HARD_RESET) {
		data_cnt = sizeof(uint32_t) * PD_HEADER_CNT(header);

		packet.cnt = data_cnt + sizeof(uint16_t);
		packet.msg_header = header;

		if (data_cnt > 0)
			memcpy(packet.data, (uint8_t *) data, data_cnt);

		rv = sgmdev_pd_write_reg(TCPC_V10_REG_TX_BYTE_CNT, packet.cnt+1, (uint8_t *) &packet);
		if (rv)
			return rv;
	}

	rv = sgmdev_i2c_write8(TCPC_V10_REG_TRANSMIT,
			TCPC_V10_REG_TRANSMIT_SET(p_tcpc_dev->pd_retry_count, type));

	return rv;
}

static int sgm41620_set_bist_test_mode( bool en)
{
	uint8_t data;
	int ret;
#if SG41620_C_PATCH == 0
    uint32_t alert_mask;

    ret = tcpci_get_alert_mask(p_tcpc_dev, &alert_mask);
    if (ret)
        return ret;

    // mask rx alert in BIST mode
    if (en) {
        alert_mask &= ~TCPC_REG_ALERT_RX_STATUS;
    } else {
        // It shall be done to clear rx buffer by W1C to rx_sop_msg
        // before enable Rx Status interrupt. If not, once exiting
        // BIST mode, previous PD_DATA_BIST message stored in the 
        // buffer by HW will trigger Rx Status Alert, resulting in
        // entering BIST mode again (PD_BIST_MODE_EVENT_PENDING).
        ret = sgm41620_alert_status_clear(TCPC_REG_ALERT_RX_STATUS);
        if (ret)
            return ret;
        alert_mask |= TCPC_REG_ALERT_RX_STATUS;
    }

    ret = tcpci_set_alert_mask(p_tcpc_dev, alert_mask);
    if (ret)
        return ret;
#endif

	ret = sgmdev_i2c_read8(TCPC_V10_REG_TCPC_CTRL,&data);
	if (ret)
		return ret;

	data &= ~TCPC_V10_REG_TCPC_CTRL_BIST_TEST_MODE;
	data |= en ? TCPC_V10_REG_TCPC_CTRL_BIST_TEST_MODE : 0;

	return sgmdev_i2c_write8(TCPC_V10_REG_TCPC_CTRL, data);
}

struct tcpc_ops sgm41620_tcpc_ops = {
	.init = sgm41620_tcpc_init,
	.init_alert_mask = sgm41620_init_alert_mask,
	.alert_status_clear = sgm41620_alert_status_clear,
	.fault_status_clear = sgm41620_fault_status_clear,
	.get_alert_mask = sgm41620_get_alert_mask,
	.set_alert_mask = sgm41620_set_alert_mask,
	.get_alert_status = sgm41620_get_alert_status,
	.get_power_status = sgm41620_get_power_status,
	.get_fault_status = sgm41620_get_fault_status,
	.get_cc = sgm41620_get_cc,
	.set_cc = sgm41620_set_cc,
	.set_polarity = sgm41620_set_polarity,
	.set_low_rp_duty = sgm41620_set_low_rp_duty,
	.set_vconn = sgm41620_set_vconn,
	.deinit = sgm41620_tcpc_deinit,

#if CONFIG_TCPC_LOW_POWER_MODE
	.set_low_power_mode = sgm41620_set_low_power_mode,
#endif	/* CONFIG_TCPC_LOW_POWER_MODE */

	.set_msg_header = sgm41620_set_msg_header,
	.set_rx_enable = sgm41620_set_rx_enable,
	.get_message = sgm41620_get_message,
	.transmit = sgm41620_transmit,
	.set_bist_test_mode = sgm41620_set_bist_test_mode,
	.set_bist_carrier_mode = sgm41620_set_bist_carrier_mode,
	.retransmit = sgm41620_retransmit,
	.set_force_discharge = sgm41620_set_force_discharge,
    .set_vbus = sgm41620_set_vbus,
};

#elif CHIP_TYPE == CHIP_SG795
static int sgm795_init_power_status_mask(void)
{
    const uint8_t mask = TCPC_V10_REG_POWER_STATUS_VBUS_PRES;

    return sgmdev_i2c_write8(TCPC_V10_REG_POWER_STATUS_MASK, mask);
}

static int sgm795_init_fault_mask(void)
{
    const uint8_t mask = TCPC_V10_REG_FAULT_STATUS_ALLRST | \
                         TCPC_V10_REG_FAULT_STATUS_VCONN_OC;

    return sgmdev_i2c_write8(TCPC_V10_REG_FAULT_STATUS_MASK, mask);
}

static int sgm795_init_sgm_mask(void)
{
    uint8_t sgm_mask = 0;

    sgm_mask |= SGM795_REG_SGINT_VSAFE0V \
              | SGM795_REG_SGINT_WAKEUP \
              | SGM795_REG_SGINT_VCONNOTP \
              | SGM795_REG_SGINT_VCONNOV;

    return sgmdev_i2c_write8(SGM795_REG_SGINT_MASK, sgm_mask);
}


static int sgm795_tcpc_init(bool sw_reset)
{
    /* UFP Both RD setting */
    /* DRP = 0, RpVal = 0 (Default), Rd, Rd */
    sgmdev_i2c_write8(TCPC_V10_REG_ROLE_CTRL, 
        TCPC_V10_REG_ROLE_CTRL_RES_SET(0, 0, CC_RD, CC_RD));

    /*
     * DRP Toggle Cycle : 51.2 + 6.4*val ms
     * DRP Duty Ctrl : dcSRC: /1024
     */
    sgmdev_i2c_write8(SGM795_REG_DRP_TOGGLE_CYCLE, 4);
    sgmdev_i2c_write16(SGM795_REG_DRP_DUTY_CTRL0, TCPC_NORMAL_RP_DUTY & 0xFF);
    sgmdev_i2c_write16(SGM795_REG_DRP_DUTY_CTRL1, (TCPC_NORMAL_RP_DUTY >> 8) & 0x3);

    tcpci_fault_status_clear(p_tcpc_dev, 0xFF);
    tcpci_alert_status_clear(p_tcpc_dev, 0xFFFFFFFF);

    sgm795_init_power_status_mask();
    tcpci_init_alert_mask(p_tcpc_dev);
    sgm795_init_fault_mask();
    sgm795_init_sgm_mask();

    // auto enable vbus detect in non-shutdown mode
    sgmdev_i2c_write8(SGM795_REG_SHUTDOWN, SGM795_REG_SHUTDOWN_OFF);
    return 0;
}

static int sgm795_init_alert_mask(void)
{
    uint16_t mask;

    mask = TCPC_V10_REG_ALERT_CC_STATUS | TCPC_V10_REG_ALERT_POWER_STATUS;

    mask |= TCPC_V10_REG_ALERT_TX_SUCCESS \
            | TCPC_V10_REG_ALERT_TX_DISCARDED \
            | TCPC_V10_REG_ALERT_TX_FAILED \
            | TCPC_V10_REG_ALERT_RX_HARD_RST \
            | TCPC_V10_REG_ALERT_RX_STATUS \
            | TCPC_V10_REG_RX_OVERFLOW;

    mask |= TCPC_REG_ALERT_FAULT | TCPC_V10_REG_ALERT_EXT_STATUS;

    return sgmdev_i2c_write16(TCPC_V10_REG_ALERT_MASK, mask);
}

static int sgm795_alert_status_clear(uint32_t mask)
{
    int ret;
    uint16_t mask_t1 = 0;
    uint8_t mask_t2 = 0;

    /* Write 1 clear */
    mask_t1 = (uint16_t) mask;
    //log_i("after mask_t1 = 0x%x, mask = %x\r\n",mask_t1,mask);
    if (mask_t1) {
        ret = sgmdev_i2c_write16(TCPC_V10_REG_ALERT, mask_t1);
        if (ret)
            return ret;
    }

    mask_t2 = mask >> 16;
    if (mask_t2) {
        ret = sgmdev_i2c_write8(SGM795_REG_SGINT, mask_t2);
        if (ret )
            return ret;
    }

    return 0;
}

static int sgm795_fault_status_clear(uint8_t status)
{
    return sgmdev_i2c_write8(TCPC_V10_REG_FAULT_STATUS, status);
}

static int sgm795_get_alert_mask(uint32_t *mask)
{
    int ret;
    uint16_t buf;
    uint8_t v2;

    ret = sgmdev_i2c_read16(TCPC_V10_REG_ALERT_MASK, &buf);
    if (ret)
        return ret;
    *mask = buf;

    ret = sgmdev_i2c_read8(SGM41620_REG_ANAMASK, &v2);
    if (ret)
        return ret;

    *mask |= v2 << 16;

    return 0;
}

static int sgm795_set_alert_mask(uint32_t mask)
{
    return sgmdev_i2c_write16(TCPC_V10_REG_ALERT_MASK, mask);
}

static int sgm795_get_alert_status(uint32_t *alert)
{
    int ret;
    uint16_t buf;
    uint8_t v2;

    ret = sgmdev_i2c_read16(TCPC_V10_REG_ALERT, &buf);
    if (ret) {
        log_e("%s get alert_status fail ret =  0x%x\n",__func__,ret);
        return ret;
    }
    *alert = buf;

    ret = sgmdev_i2c_read8(SGM795_REG_SGINT, &v2);
    if (ret) {
        log_e("%s get rt_int fail ret =  0x%x\n",__func__,ret);
        return ret;
    }
    *alert |= v2 << 16;

    return 0;
}

static int sgm795_get_power_status(uint16_t *pwr_status)
{
    int ret;
    uint8_t data;

    ret = sgmdev_i2c_read8(TCPC_V10_REG_POWER_STATUS, &data);
    if (ret) {
        log_e("%s get power status fail ret =  0x%x\n",__func__,ret);
        return ret;
    }
    *pwr_status = 0;

    if (data & TCPC_V10_REG_POWER_STATUS_VBUS_PRES)
        *pwr_status |= TCPC_REG_POWER_STATUS_VBUS_PRES;

    ret = sgmdev_i2c_read8(SGM795_REG_SGSTS, &data);
    if (ret) {
        log_e("%s get rt status fail ret =  0x%x\n",__func__,ret);
        return ret;
    }

    if (data & SGM795_REG_SGSTS_VSAFE0V)
        *pwr_status |= TCPC_REG_POWER_STATUS_EXT_VSAFE0V;

    return 0;
}

static int sgm795_get_fault_status(uint8_t *status)
{
    int ret;
    uint8_t buff;

    ret = sgmdev_i2c_read8(TCPC_V10_REG_FAULT_STATUS, &buff);
    if (ret) {
        log_e("%s get fault status fail ret =  0x%x\n",__func__,ret);
        return ret;
    }
    *status = buff;

    return 0;
}

static int sgm795_get_cc( int *cc1, int *cc2)
{
    int ret; 
    uint8_t status, role_ctrl, cc_role;
    bool act_as_drp, act_as_sink;

    ret = sgmdev_i2c_read8(TCPC_V10_REG_CC_STATUS, &status);
    if (ret) {
        log_e("%s get cc status fail ret =  0x%x\n",__func__,ret);
        return ret;
    }

    ret = sgmdev_i2c_read8(TCPC_V10_REG_ROLE_CTRL, &role_ctrl);
    if (ret) {
        log_e("%s get role fail ret =  0x%x\n",__func__,ret);
        return ret;
    }

    if (status & TCPC_V10_REG_CC_STATUS_DRP_TOGGLING) {
        *cc1 = TYPEC_CC_DRP_TOGGLING;
        *cc2 = TYPEC_CC_DRP_TOGGLING;
        return 0;
    }

    *cc1 = TCPC_V10_REG_CC_STATUS_CC1(status);
    *cc2 = TCPC_V10_REG_CC_STATUS_CC2(status);

    act_as_drp = TCPC_V10_REG_ROLE_CTRL_DRP & role_ctrl;

    if (act_as_drp) {
        act_as_sink = TCPC_V10_REG_CC_STATUS_DRP_RESULT(status);
    } 
    else {
        if (p_tcpc_dev->typec_polarity)
            cc_role = TCPC_V10_REG_CC_STATUS_CC2(role_ctrl);
        else
            cc_role = TCPC_V10_REG_CC_STATUS_CC1(role_ctrl);

        if (cc_role == TYPEC_CC_RP)
            act_as_sink = false;
        else
            act_as_sink = true;
    }

    /*
     * If status is not open, then OR in termination to convert to
     * enum tcpc_cc_voltage_status.
     */
    if (*cc1 != TYPEC_CC_VOLT_OPEN)
        *cc1 |= (act_as_sink << 2);

    if (*cc2 != TYPEC_CC_VOLT_OPEN)
        *cc2 |= (act_as_sink << 2);

    return 0;
}

static int sgm795_enable_vsafe0v_detect(bool enable)
{
    uint8_t data;
    int ret;

    ret = sgmdev_i2c_read8(SGM795_REG_SGINT_MASK, &data);
    if (ret)
        return ret;

    if (enable)
        data |= SGM795_REG_SGINT_VSAFE0V;
    else
        data &= ~SGM795_REG_SGINT_VSAFE0V;
    sgmdev_i2c_write8(SGM795_REG_SGINT_MASK, data);

    sgmdev_command(TCPM_CMD_ENABLE_VBUS_DETECT);
    return 0;
}

static int sgm795_set_cc(int pull)
{
    int ret;
    uint8_t data;
    int rp_lvl = TYPEC_CC_PULL_GET_RP_LVL(pull), pull1, pull2;

    pull = TYPEC_CC_PULL_GET_RES(pull);
    if (pull == TYPEC_CC_DRP) {
        data = TCPC_V10_REG_ROLE_CTRL_RES_SET(1, rp_lvl, TYPEC_CC_RD, TYPEC_CC_RD);

        ret = sgmdev_i2c_write8(TCPC_V10_REG_ROLE_CTRL, data);

        if (ret == 0) {
            sgm795_enable_vsafe0v_detect(false);
            ret = sgmdev_command(TCPM_CMD_LOOK_CONNECTION);
        }
    } else {
        pull1 = pull2 = pull;

        if (pull == TYPEC_CC_RP && p_tcpc_dev->typec_is_attached_src) {
            if (p_tcpc_dev->typec_polarity)
                pull1 = TYPEC_CC_OPEN;
            else
                pull2 = TYPEC_CC_OPEN;
        }

        data = TCPC_V10_REG_ROLE_CTRL_RES_SET(0, rp_lvl, pull1, pull2);
        ret = sgmdev_i2c_write8(TCPC_V10_REG_ROLE_CTRL, data);
    }

    return 0;
}

static int sgm795_set_polarity(int polarity)
{
    int ret = 0;
    uint8_t data;

    ret = sgmdev_i2c_read8(TCPC_V10_REG_TCPC_CTRL, &data);
    if (ret)
        return ret;

    data &= ~TCPC_V10_REG_TCPC_CTRL_PLUG_ORIENT;
    data |= polarity ? TCPC_V10_REG_TCPC_CTRL_PLUG_ORIENT : 0;

    return sgmdev_i2c_write8(TCPC_V10_REG_TCPC_CTRL, data);
}

static int sgm795_set_low_rp_duty(bool low_rp)
{
    if (low_rp) {
        return sgmdev_i2c_write16(SGM41620_REG_DRP_DUTY_CTRL0, TCPC_LOW_RP_DUTY);
    } else {
        sgmdev_i2c_write16(SGM41620_REG_DRP_DUTY_CTRL0, TCPC_NORMAL_RP_DUTY & 0xFF);
        return sgmdev_i2c_write16(SGM41620_REG_DRP_DUTY_CTRL1, (TCPC_NORMAL_RP_DUTY >> 8) & 0x3);
    }
}

static int sgm795_set_vconn(int enable)
{
    int rv;
    int ret;
    uint8_t data;

    ret = sgmdev_i2c_read8(TCPC_V10_REG_POWER_CTRL, &data);
    if (ret) {
        log_e("%s get power fail ret =  0x%x\n",__func__,ret);
        return ret;
    }

    data &= ~TCPC_V10_REG_POWER_CTRL_VCONN;
    data |= enable ? TCPC_V10_REG_POWER_CTRL_VCONN : 0;

    rv = sgmdev_i2c_write8(TCPC_V10_REG_POWER_CTRL, data);
    if (ret) {
        log_e("%s set power fail ret =  0x%x\n",__func__,ret);
        return ret;
    }

#ifndef CONFIG_TCPC_IDLE_MODE
    rv = sgmdev_i2c_write8(SGM41620_REG_IDLE_CTRL,
        SGM41620_REG_IDLE_SET(0, 1, enable ? 0 : 1, 2));
#endif /* CONFIG_TCPC_IDLE_MODE */

    return rv;
}

static int sgm795_tcpc_deinit(void)
{
#if CONFIG_TCPC_SHUTDOWN_CC_DETACH
    sgm795_set_cc(TYPEC_CC_DRP);
    sgm795_set_cc(TYPEC_CC_OPEN);

    sgmdev_i2c_write16(TCPC_V10_REG_ALERT_MASK, 0);
    sgmdev_i2c_write16(TCPC_V10_REG_POWER_STATUS_MASK, 0);
    sgmdev_i2c_write16(SGM795_REG_SGINT_MASK, 0);
#else
    sgmdev_software_reset();
#endif	/* CONFIG_TCPC_SHUTDOWN_CC_DETACH */

    return 0;
}

static int sgm795_set_low_power_mode(bool en, int pull)
{
    int ret;
    uint8_t data;

    ret = sgmdev_i2c_read8(SGM795_REG_SGCTL, &data);
    if (ret)
        return ret;

    if (en) {
        data |= SGM795_REG_SGCTL_LPEN;
        if (pull == TYPEC_CC_RP)
            data |= SGM795_REG_SGCTL_LPRPRD;
        else
            data &= ~SGM795_REG_SGCTL_LPRPRD;
    } else {
        data &= ~(SGM795_REG_SGCTL_LPEN | SGM795_REG_SGCTL_LPRPRD);
    
        sgm795_enable_vsafe0v_detect(true);
    }

    ret = sgmdev_i2c_write8(SGM795_REG_SGCTL, &data);
    if (ret)
        return ret;

    return 0;
}

static int sgm795_set_msg_header(uint8_t power_role, uint8_t data_role)
{
    uint8_t msg_hdr = TCPC_V10_REG_MSG_HDR_INFO_SET(data_role, power_role);

    return sgmdev_i2c_write8(TCPC_V10_REG_MSG_HDR_INFO, msg_hdr);
}

static int sgm795_set_rx_enable(uint8_t enable)
{
    int ret = 0;

    ret = sgmdev_i2c_write8(TCPC_V10_REG_RX_DETECT, enable);

    if ((ret == 0) && (!enable)) {
        ret = sgm795_alert_status_clear(TCPC_REG_ALERT_RX_STATUS | \
                    TCPC_REG_ALERT_RX_HARD_RST | \
                    TCPC_REG_ALERT_RX_BUF_OVF);
    }

    return ret;
}

static int sgm795_get_message( uint32_t *payload,
        uint16_t *msg_head, enum tcpm_transmit_type *frame_type)
{
    int rv;
    uint8_t type, cnt = 0;
    uint8_t buf[4];
    const uint16_t alert_rx =
        TCPC_V10_REG_ALERT_RX_STATUS | TCPC_V10_REG_RX_OVERFLOW;

    rv = sgmdev_pd_read_reg(TCPC_V10_REG_RX_BYTE_CNT, 4, buf);
    cnt = buf[0];
    type = buf[1];
    *msg_head = *(uint16_t *)&buf[2];

    /* TCPC 1.0 ==> no need to subtract the size of msg_head */
    if (!rv && cnt > 3) {
        cnt -= 3; /* MSG_HDR */
        rv = sgmdev_pd_read_reg(TCPC_V10_REG_RX_DATA, cnt,(uint8_t *) payload);
    }

    *frame_type = (enum tcpm_transmit_type) type;

    /* Read complete, clear RX status alert bit */
    tcpci_alert_status_clear(p_tcpc_dev, alert_rx);

    return rv;
}

static int sgm795_transmit(enum tcpm_transmit_type type, uint16_t header, const uint32_t *data)
{
    int rv;
    int data_cnt;
    struct tcpc_transmit_packet packet;

    if (type < TCPC_TX_HARD_RESET) {
        data_cnt = sizeof(uint32_t) * PD_HEADER_CNT(header);

        packet.cnt = data_cnt + sizeof(uint16_t);
        packet.msg_header = header;

        if (data_cnt > 0)
        memcpy(packet.data, (uint8_t *) data, data_cnt);

        rv = sgmdev_pd_write_reg(TCPC_V10_REG_TX_BYTE_CNT, packet.cnt+1, (uint8_t *) &packet);
        if (rv)
            return rv;
    }

    rv = sgmdev_i2c_write8(TCPC_V10_REG_TRANSMIT,
        TCPC_V10_REG_TRANSMIT_SET(p_tcpc_dev->pd_retry_count, type));

    return rv;
}

static int sgm795_set_bist_test_mode( bool en)
{
    uint8_t data;
    int ret;

    ret = sgmdev_i2c_read8(TCPC_V10_REG_TCPC_CTRL,&data);
    if (ret)
        return ret;

    data &= ~TCPC_V10_REG_TCPC_CTRL_BIST_TEST_MODE;
    data |= en ? TCPC_V10_REG_TCPC_CTRL_BIST_TEST_MODE : 0;

    sgmdev_i2c_write8(TCPC_V10_REG_ALERT_MASK, en ? 0x7B : 0x7F);

    return sgmdev_i2c_write8(TCPC_V10_REG_TCPC_CTRL, data);
}

static int sgm795_set_bist_carrier_mode(uint8_t pattern)
{
    /* Don't support this function */
    return 0;
}

static int sgm795_retransmit(void)
{
    return sgmdev_i2c_write8(TCPC_V10_REG_TRANSMIT,
        TCPC_V10_REG_TRANSMIT_SET(p_tcpc_dev->pd_retry_count, TCPC_TX_SOP));
}

static int sgm795_set_force_discharge(bool en, int mv)
{
    int ret;
    uint8_t data;

    ret = sgmdev_i2c_read8(TCPC_V10_REG_POWER_CTRL, &data);
    if (ret < 0)
        return ret;
    
    data &= ~TCPC_V10_REG_FORCE_DISC_EN;
    data |= en ? TCPC_V10_REG_FORCE_DISC_EN : 0;

    return sgmdev_i2c_write8(TCPC_V10_REG_POWER_CTRL, data);
}

static int sgm795_set_vbus(bool en)
{
    // TODO: control charger ic to source vbus
    return 0; 
}

struct tcpc_ops sgm795_tcpc_ops = {
    .init = sgm795_tcpc_init,
    .init_alert_mask = sgm795_init_alert_mask,
    .alert_status_clear = sgm795_alert_status_clear,
    .fault_status_clear = sgm795_fault_status_clear,
    .get_alert_mask = sgm795_get_alert_mask,
    .set_alert_mask = sgm795_set_alert_mask,
    .get_alert_status = sgm795_get_alert_status,
    .get_power_status = sgm795_get_power_status,
    .get_fault_status = sgm795_get_fault_status,
    .get_cc = sgm795_get_cc,
    .set_cc = sgm795_set_cc,
    .set_polarity = sgm795_set_polarity,
    .set_low_rp_duty = sgm795_set_low_rp_duty,
    .set_vconn = sgm795_set_vconn,
    .deinit = sgm795_tcpc_deinit,
    .set_low_power_mode = sgm795_set_low_power_mode,

    .set_msg_header = sgm795_set_msg_header,
    .set_rx_enable = sgm795_set_rx_enable,
    .get_message = sgm795_get_message,
    .transmit = sgm795_transmit,
    .set_bist_test_mode = sgm795_set_bist_test_mode,
    .set_bist_carrier_mode = sgm795_set_bist_carrier_mode,
    .retransmit = sgm795_retransmit,
    .set_force_discharge = sgm795_set_force_discharge,
    .set_vbus = sgm795_set_vbus,
};
#endif // CHIP_TYPE

void tcpc_device_wrap_ops(void)
{
#if CHIP_TYPE == CHIP_SG41620
    p_tcpc_dev->ops = &sgm41620_tcpc_ops;
#elif CHIP_TYPE == CHIP_SG795
    p_tcpc_dev->ops = &sgm795_tcpc_ops;
#endif
}

